#ifndef CPATHPLANRRT_H
#define CPATHPLANRRT_H

#include <QObject>
#include <QPoint>
#include <QVector>
#include <memory>
#include "CGridMap.h"

#include "RRT_Type.h"
#include "CRRTTree.h"

class CPathPlanRRT : public QObject
{
    Q_OBJECT
private:

public:
    CPathPlanRRT(QObject *parent = nullptr, double threshold = 10.0, double close_goal_dis = 1.0);

    CPathPlanRRT(double threshold = 10.0, double close_goal_dis = 1.0);

    ~CPathPlanRRT();

    void updateMap(const CGridMap &mapData);

    void setStartPoint(QPoint pt)  {   m_start_point = pt;  }
    void setDestPoint(QPoint pt)   {   m_dest_point = pt;   }

    bool planning();

private:

    bool isOccupied(const CPoint2D &point);

    bool isOutOfBound(const CPoint2D &point);

    bool checkSegmentCollision(const CPoint2D &start_point, const CPoint2D &end_point);

    bool isClosedDestPoint(const CPoint2D &point);

signals:

    void signalDisplayRandomPoint(QPoint point);

    void signalDisplayTreeNode(TNode node);

    void signalDisplayOriginPath(QVector<QPoint>);

private:

    quint16  m_size_x;
    quint16  m_size_y;

    int      m_step;

    double   m_threhold;
    double   m_close_goal_distance;

    QPoint   m_start_point;
    QPoint   m_dest_point;

    QVector<quint8>  m_map_byte;

    CRRTTree * m_rrt_tree;
};

#endif // CPATHPLANRRT_H
